#include <ros/ros.h>
#include "viobot.h"
#include <signal.h>

VioBot VIOBOT;

bool break_flag = false;

void mySigintHandler(int sig)
{
    break_flag = true;
    VIOBOT.shutdown_stereo2();
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "viobot_node");
    ros::NodeHandle nh("~");
    ros::Rate rate(100.0);

    signal(SIGINT, mySigintHandler);
    ros::Duration(1.0).sleep();

    VIOBOT.init(nh, false);

    VIOBOT.start_stereo2();

    // 主循环
    while (ros::ok()) {

        if(break_flag)
            break;

        // 回调函数
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}
